#!/usr/bin/env python
#-*-coding:utf-8-*-


import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist, Transform, TransformStamped
from gazebo_msgs.msg import LinkStates
from std_msgs.msg import Header
import numpy as np
import math
import tf2_ros
class OdometryNode:
    # Set publishers 设置发布话题
    pub_odom = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    def __init__(self):
        # init internals 初始化内部
        self.last_received_pose = Pose()
        self.last_received_twist = Twist()
        self.last_recieved_stamp = None
        # Set subscribers 设置订阅话题
        rospy.Subscriber('/gazebo/link_states', LinkStates, self.sub_robot_pose_update)

    def sub_robot_pose_update(self, msg):
        cmd = Twist()
        global  goal_posex
        global  goal_posey 
        goal_posex = 0.00
        goal_posey = -5.2
        # Find the index of the racecar 找到赛车的索引
        try:
            #创建msg消息对象
            arrayIndex = msg.name.index('robot::dummy')
        except ValueError as e:
            # Wait for Gazebo to startup 等待Gazebo启动
            pass
        else:
            # Extract our current position information 提取我们当前的位置信息
            self.last_received_pose = msg.pose[arrayIndex]
            self.last_received_twist = msg.twist[arrayIndex]
            if (abs(self.last_received_pose.position.x - goal_posex)<0.3) and (abs(self.last_received_pose.position.y - goal_posey)<0.3):
                cmd .linear.x = 0
                cmd .angular.z = 0
                self.pub_odom.publish(cmd)
                
# Start the node
if __name__ == '__main__':
    rospy.init_node("stop_car")#建立节点
    #调用类。在类调用类的同时调用了发送tf和topic的函数
    OdometryNode()
    #等待阻塞
    rospy.spin()
